/*
 * hal_current_sensor.c
 *
 *  Created on: 2025 Apr 9
 *      Author: SFLY
 */
#include "hal_current_sensor.h"

void Hal_Current_Sensor_Init(xt_hal_current_sensor_t* sensor)
{
    sensor->IuvwGain             = __builtin_dsp_fsdiv_qx320f(ADC_VOLT_REF, ((float32_t)ADC_MAX_RANGE)*Current_AmplifierRs*Current_AmplifierGain*I_BASE);
    sensor->VbusGain             = __builtin_dsp_fsdiv_qx320f(ADC_VOLT_REF*Vbus_Gain, ((float32_t)ADC_MAX_RANGE)*V_BASE);
    sensor->OffsetDetectTime  = Current_OffsetCheckTs * CurrentSampleFs * 0.001f;
    sensor->IuOffset          = 0;
    sensor->IvOffset          = 0;
    sensor->IwOffset          = 0;
    sensor->OffsetDetectCnt   = 0;
    sensor->CurrentSensorRdy  = FALSE;
    sensor->current_sensor_update = Hal_Current_Sensor_Update;
    sensor->current_sensor_checkoffset = Hal_Current_Sensor_CheckOffset;
    //首先采集一下母线电压
    sensor->StdVbusObser = Bsp_Get_Vbus_Adc_Value()* sensor->VbusGain;
    sensor->RVbusObser =  Bsp_Get_Vbus_Adc_Value()*sensor->VbusGain*V_BASE;
}

void Hal_Current_Sensor_Update(void* sensor_void_ptr)
{
    xt_hal_current_sensor_t* sensor = (xt_hal_current_sensor_t*)sensor_void_ptr;
    uint32_t iu_value = Bsp_Get_Iu_Adc_Value();
    uint32_t iv_value = Bsp_Get_Iv_Adc_Value();
    uint32_t iw_value = Bsp_Get_Iw_Adc_Value();
    sensor->StdVbusObser = Bsp_Get_Vbus_Adc_Value()* sensor->VbusGain;
    sensor->RVbusObser =  Bsp_Get_Vbus_Adc_Value()*sensor->VbusGain*V_BASE;
    sensor->IuObser  = ( iu_value-sensor->IuOffset)*sensor->IuvwGain;
    sensor->IvObser  =  (iv_value-sensor->IvOffset)*sensor->IuvwGain;
    sensor->IwObser = -(sensor->IuObser+ sensor->IvObser);
    //sensor->IwObser  =  (iw_value-sensor->IwOffset)*sensor->IuvwGain;
}

void Hal_Current_Sensor_CheckOffset(void* sensor_void_ptr)
{
    xt_hal_current_sensor_t* sensor = (xt_hal_current_sensor_t*)sensor_void_ptr;
    sensor->StdVbusObser = Bsp_Get_Vbus_Adc_Value()* sensor->VbusGain;
    sensor->RVbusObser =  Bsp_Get_Vbus_Adc_Value()*sensor->VbusGain*V_BASE;
    if (sensor->OffsetDetectCnt < sensor->OffsetDetectTime) {
        sensor->IuOffset += Bsp_Get_Iu_Adc_Value();
        sensor->IvOffset += Bsp_Get_Iv_Adc_Value();
        sensor->IwOffset += Bsp_Get_Iw_Adc_Value();
        sensor->OffsetDetectCnt++;
    } else {
        sensor->IuOffset = __builtin_dsp_fsdiv_qx320f(sensor->IuOffset, sensor->OffsetDetectCnt);
        sensor->IvOffset = __builtin_dsp_fsdiv_qx320f(sensor->IvOffset, sensor->OffsetDetectCnt);
        sensor->IwOffset = __builtin_dsp_fsdiv_qx320f(sensor->IwOffset, sensor->OffsetDetectCnt);
        sensor->CurrentSensorRdy = TRUE;
    }
}

